#include "maixcam.h"
#include "uartx.h"
#include "mypid.h"
#include "maixcam.h"

#define Per_angle 5.55555
uint16_t Maixcam_data[10]={0};
int16_t PWM;
int16_t maixcam_AngletoPMW(float angle){
	
	PWM = 1500+(int16_t)(angle*Per_angle);
	return PWM;
	
}

uint8_t flag = 0;

void maixcam_update(void)
{
	if(u2->Cplt==1)
		{
			u2->Cplt=0;
			Maixcam_data[0]=SringToUint16(u2->RxBuffer+2);
			X_1=Maixcam_data[0];
			Maixcam_data[1]=SringToUint16(u2->RxBuffer+6);
			Y_1=Maixcam_data[1];
			Maixcam_data[2]=SringToUint16(u2->RxBuffer+10);
			X_2=Maixcam_data[2];
			Maixcam_data[3]=SringToUint16(u2->RxBuffer+14);
			Y_2=Maixcam_data[3];
			Maixcam_data[4]=SringToUint16(u2->RxBuffer+18);
			X_3=Maixcam_data[4];
			Maixcam_data[5]=SringToUint16(u2->RxBuffer+22);
			Y_3=Maixcam_data[5];
			Maixcam_data[6]=SringToUint16(u2->RxBuffer+26);
			X_4=Maixcam_data[6];
			Maixcam_data[7]=SringToUint16(u2->RxBuffer+30);
			Y_4=Maixcam_data[7];
			Maixcam_data[8]=SringToUint16(u2->RxBuffer+34);
			Laser_x=Maixcam_data[8];
			Maixcam_data[9]=SringToUint16(u2->RxBuffer+38);
			Laser_y=Maixcam_data[9];
			
			if(flag == 0)
			{
				flag = 1;
				pid1.Target = X_1;
				pid2.Target = Y_1;
			}
		}
}
